#include "BestPath.h"

std::vector<Point*> BestPath::best_path;
void BestPath::find_best_path(Point* Source, Point* To) {
	best_path.clear();
	if (Source == To || Source == nullptr || To == nullptr) {
		return;
	}
	auto G = *Edge::graph();
	int n = G.size() - 1;
	std::vector<int> dis(n + 1, -1);
	std::vector<bool> vis(n + 1);
	std::priority_queue<std::pair<int, int>, std::vector<std::pair<int, int>>, std::greater<std::pair<int, int>>> q;
	std::vector<std::vector<std::pair<int, int>>>g(n + 1);
	int s = (*Point::map())[Source];
	int t = (*Point::map())[To];
	std::vector<int>pre(n + 1, -1);

	for (auto& a : G) {
		for (auto& b : a.second) {
			g[(*Point::map())[a.first]].emplace_back(std::make_pair((*Point::map())[b->get_p2()], b->get_cost()));
		}
	}

	dis[s] = 0;
	q.emplace(std::make_pair(0, s));
	while (!q.empty())
	{
		int u = q.top().second;
		q.pop();
		if (vis[u])
			continue;
		vis[u] = 1;
		if (u == t)break;
		for (auto& a : g[u])
		{
			auto to = a.first;
			auto w = a.second;
			if (dis[to] > dis[u] + w || dis[to] == -1)
			{
				dis[to] = dis[u] + w;
				pre[to] = u;
				q.emplace(std::make_pair(dis[to], to));
			}
		}
	}
	int tr = t;
	while (tr != -1) {
		best_path.emplace_back((*Point::vector())[tr]);
		tr = pre[tr];
	}
	return;
}
void BestPath::find_shortest_path(Point* Source, Point* To) {
	best_path.clear();
	if (Source == To || Source == nullptr || To == nullptr) {
		return;
	}
	auto G = *Edge::graph();
	int n = G.size() - 1;
	std::vector<int> dis(n + 1, -1);
	std::vector<bool> vis(n + 1);
	std::priority_queue<std::pair<int, int>, std::vector<std::pair<int, int>>, std::greater<std::pair<int, int>>> q;
	std::vector<std::vector<std::pair<int, int>>>g(n + 1);
	int s = (*Point::map())[Source];
	int t = (*Point::map())[To];
	std::vector<int>pre(n + 1,-1);

	for (auto& a : G) {
		for (auto& b : a.second) {
			g[(*Point::map())[a.first]].emplace_back(std::make_pair((*Point::map())[b->get_p2()], b->get_len()));
		}
	}

	dis[s] = 0;
	q.emplace(std::make_pair(0, s));
	while (!q.empty())
	{
		int u = q.top().second;
		q.pop();
		if (vis[u])
			continue;
		vis[u] = 1;
		if (u == t)break;
		for (auto& a : g[u])
		{
			auto to = a.first;
			auto w = a.second;
			if (dis[to] > dis[u] + w || dis[to] == -1)
			{
				dis[to] = dis[u] + w;
				pre[to] = u;
				q.emplace(std::make_pair(dis[to], to));
			}
		}
	}
	int tr = t;
	while (tr != -1) {
		best_path.emplace_back((*Point::vector())[tr]);
		tr = pre[tr];
	}
	return;
}
std::vector<Point*> BestPath::get_best_path() {
	return best_path;
}